/*
 * Author  : Leo <lipf160627@163.com>
 * Created : 2025-7-27
 * Desc    : Co-mcu using USART to communicate with STM32
 */

#pragma once

#include <array>
#include <stdint.h>

#include "driver/gpio.h"
#include "driver/uart.h"

// 通信协议
// 帧头(2) + 命令符(1) + 数据(4) + 校验码(1)
// 校验码采用XOR：^帧头 + ^命令符 + ^数据1 + ^数据2 + ^数据3 + ^数据4
// 帧头: 0xAA 0x55
// 命令符 -- 含义 ------------ 数据1 ------- 数据2 -------------------- 数据3 ------------ 数据4
//  0x01  | 前进            | 距离cm      | 速度0-100，0表示当前速度
//  0x02  | 后退            | 距离cm      | 速度0-100，0表示当前速度
//  0x03  | 左转            | 角度0-180   | 角速度1-180°/s
//  0x04  | 右转            | 角度0-180   | 角速度1-180°/s
//  0x05  | 刹车            |
//  0xFE  | 强制停止当前任务 |
//  0xE0  | 结果应答        | 收到的命令符 | 0：成功，1：失败
//  0xE1  | 状态应答        | 收到的命令符 | 已完成距离/角度等         | 剩余距离/角度等

#define CO_MCU_REQUEST_SIZE     8
#define CO_MCU_RESPONSE_SIZE    8

#define CO_MCU_CMD_FORWARD      0x01
#define CO_MCU_CMD_BACK         0x02
#define CO_MCU_CMD_TURN_LEFT    0x03
#define CO_MCU_CMD_TURN_RIGHT   0x04
#define CO_MCU_CMD_STOP         0x05
#define CO_MCU_CMD_RESULT_ACK   0xE0
#define CO_MCU_CMD_STATUS_ACK   0xE1
#define CO_MCU_CMD_TERMINATE    0xFE

#define CO_MCU_ACK_SUCCESS      0x00
#define CO_MCU_ACK_FAIL         0x01

class CoMcu {
public:
    CoMcu(uart_port_t uartNum, gpio_num_t txPin, gpio_num_t rxPin, int baudRate);
    ~CoMcu() = default;

    bool forward(uint8_t distance, uint8_t speed);
    bool back(uint8_t distance, uint8_t speed);
    bool turnLeft(uint8_t angle, uint8_t speed);
    bool turnRight(uint8_t angle, uint8_t speed);
    bool stop();
    bool terminate();

private:
    uart_port_t m_uartNum;
    gpio_num_t  m_pinTx;
    gpio_num_t  m_pinRx;
    int         m_baudRate;

    std::array<uint8_t, CO_MCU_REQUEST_SIZE>  m_sendBuf;
    std::array<uint8_t, CO_MCU_RESPONSE_SIZE> m_recvBuf;

    void init();
    bool verifyChecksum();
    uint8_t generateChecksum();
    bool sendAndWaitForResponse(uint8_t cmd);
};
